#include "mine_remoter_ctrl.h"
#include "mine_usart2.h"
#include "mine_fly_control.h"

static MiniFlyMsg_t msg;

static QueueHandle_t minePacketQueue;
TaskHandle_t mine_remoter_ctrl_task_handler;

/**初始化配置 */
void mine_remoter_ctrl_init(void)
{
    printf("mine_remoter_ctrl_init:sizeof(atkp_t) = %d\n", sizeof(atkp_t)); // 32byte
    minePacketQueue = xQueueCreate(7, sizeof(atkp_t));
    if (minePacketQueue == NULL)
    {
        // 队列创建失败，处理错误
        printf("队列创建失败\n");
    }
    else
    {
        // 队列创建成功
        printf("队列创建成功\n");
    }
}

/**
 * 打印任务
 */
void mine_remoter_ctrl_task(void *pvParameters)
{
    mine_remoter_ctrl_init();
    
    atkp_t pk;
    while (1)
    {

        if (mine_remote_get_queue(&pk))
        {
            //mine_printfPacket(&pk);
            mine_remoterCtrlProcess(&pk);
        }

        vTaskDelay(200);
    }
}

/**
 * 加入消息队列
 */
void mine_remote_add_queue(atkp_t *pk)
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE; // 必须初始化为pdFALSE
    // 尝试发送数据
    if (xQueueSendFromISR(minePacketQueue, pk, &xHigherPriorityTaskWoken) == pdPASS)
    {
        // 发送成功
        printf("Packet 加入队列,成功\n");
    }
    else
    {
        // 队列已满，丢弃数据或记录错误
        perror("Packet 加入队列,失败");
        // printf("Packet 加入队列,失败\n");
    }

    // 检查任务切换标记
    if (xHigherPriorityTaskWoken == pdTRUE)
    {
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken); // 触发任务切换
    }
}

/**
 * 从消息队列取出来
 */
uint8_t mine_remote_get_queue(atkp_t *pk)
{
    // BaseType_t xHigherPriorityTaskWoken = pdFALSE; // xHigherPriorityTaskWoken必须设置为pdFALSE
    //  BaseType_t xStatus = xQueueReceiveFromISR(minePacketQueue, &pk, &xHigherPriorityTaskWoken);
    BaseType_t xStatus = xQueueReceive(minePacketQueue, pk, portMAX_DELAY);
    if (xStatus == pdPASS)
    {
        return 1; // 处理数据
    }
    else
    {
        // 队列空时的处理（如重试或报错）
        printf("消息队列接收失败\n");
        return 0;
    }
    // BaseType_t xHigherPriorityTaskWoken = pdFALSE; //xHigherPriorityTaskWoken必须设置为pdFALSE
    // xQueueReceiveFromISR(minePacketQueue, pk, &xHigherPriorityTaskWoken);
}

/**
 * 打印消息体
 */
void mine_printfPacket(atkp_t *ppacket)
{
    atkp_t packet = *ppacket;

    //
    char *cmd = NULL;
    switch (packet.data[0])
    {
    case D_RADIO_HEARTBEAT:
        cmd = "D_RADIO_HEARTBEAT";
        break;
    case D_RADIO_GET_CONFIG:
        cmd = "D_RADIO_GET_CONFIG";
        break;
    case D_RADIO_SET_CONFIG:
        cmd = "D_RADIO_SET_CONFIG";
        break;
    case REMOTER_CMD:
        cmd = "REMOTER_CMD";
        break;
    default:
        cmd = "未知";
        break;
    }
    //
    char *msgIdStr = "";
    // if (packet.msgID == DOWN_RADIO)
    // {
    //     msgIdStr = "DOWN_RADIO";
    // }
    switch (packet.msgID)
    {
    case DOWN_RADIO:
        msgIdStr = "DOWN_RADIO";
        break;
    case DOWN_REMOTER:
        msgIdStr = "DOWN_REMOTER";
        break;
    }
    // packet.data[1]
    char *dataCmd = "";

    switch (packet.data[1])
    {
    case CMD_FLIGHT_LAND:
        dataCmd = "CMD_FLIGHT_LAND";
        break;
    case CMD_EMER_STOP:
        dataCmd = "CMD_EMER_STOP";
        break;
    case CMD_GET_MSG:
        dataCmd = "CMD_GET_MSG";
        break;
    case CMD_GET_CANFLY:
        dataCmd = "CMD_GET_CANFLY";
        break;
    case CMD_FLIP:
        dataCmd = "CMD_FLIP";
        break;
    case CMD_POWER_MODULE:
        dataCmd = "CMD_POWER_MODULE";
        break;
    case CMD_LEDRING_EFFECT:
        dataCmd = "CMD_LEDRING_EFFECT";
        break;
    default:
        dataCmd = "";
        break;
    }

    if (packet.msgID == DOWN_REMOTER)
    {
        printf("\n\t0xAA 0xAF msgID=%02x(%s) ,dataLen=%02x,packet.data[0]=%s,packet.data[1]=%s,packet.data[2]=%02x \n", packet.msgID, msgIdStr, packet.dataLen, cmd, dataCmd, packet.data[2]);
        //
        printf("\n\t");
        printf(" %02x ", packet.msgID);
        printf(" %02x ", packet.dataLen);
        for (int i = 0; i < packet.dataLen; i++)
        {
            printf(" %02x ", packet.data[i]);
        }
        //
        // printf("\n%s\n", data);

        //////
    }
}

/*返回四轴信息*/
static u8 reSendTimes = 3; /*微调重发次数*/

/*打包ATKPPacket数据通过串口DMA发送*/
static void mine_uartSendPacket(atkp_t *p)
{
    int dataSize;
    u8 cksum = 0;
    u8 sendBuffer[36];

    ASSERT(p->dataLen <= ATKP_MAX_DATA_SIZE);

    sendBuffer[0] = UP_BYTE1;
    sendBuffer[1] = UP_BYTE2;
    sendBuffer[2] = p->msgID;
    sendBuffer[3] = p->dataLen;

    memcpy(&sendBuffer[4], p->data, p->dataLen);
    dataSize = p->dataLen + 5; // 加上cksum
    /*计算校验和*/
    for (int i = 0; i < dataSize - 1; i++)
    {
        cksum += sendBuffer[i];
    }
    sendBuffer[dataSize - 1] = cksum;

    for (int i = 0; i < dataSize; i++)
    {
        mine_usart2_send(sendBuffer[i]);
    }

    /// uartslkSendDataDmaBlocking(dataSize, sendBuffer);
}

void mine_sendMsgACK(void)
{
    msg.version = 13;         // VERSION;// configParam.version;
    msg.mpu_selfTest = false; // getIsMPU9250Present();
    msg.baro_slfTest = false; // getIsBaroPresent();
    msg.isCanFly = true;      // getIsCalibrated();
    if (msg.isCanFly == true) /*校准通过之后发送微调值*/
    {
        if (reSendTimes > 0) /*微调重发次数*/
        {
            reSendTimes--;
            msg.trimPitch = 0.f; // configParam.trimP;
            msg.trimRoll = 0.f;  // configParam.trimR;
        }
    }
    msg.isLowpower = false;   // getIsLowpower();
    msg.moduleID = NO_MODULE; // getModuleID();

    atkp_t p;
    p.msgID = UP_REMOTER;
    p.dataLen = sizeof(msg) + 1;
    p.data[0] = ACK_MSG;
    memcpy(p.data + 1, &msg, sizeof(msg));
    // radiolinkSendPacketBlocking(&p);
    mine_uartSendPacket(&p);
}

/*遥控数据接收处理*/
void mine_remoterCtrlProcess(atkp_t *pk)
{
    atkp_t packet = *pk;
    if (packet.msgID == DOWN_REMOTER)
    {
        if (packet.data[1] == CMD_FLIGHT_LAND)
        {
            if (packet.data[2] == 1)
            {
                // 配置起飞
                mine_fly_control_set_fly_state(1);
                printf("mine_remoterCtrlProcess:fly_state = 1\n");
            }
            else if (packet.data[2] == 0)
            {
                // 配置起飞
                mine_fly_control_set_fly_state(0);
                printf("mine_remoterCtrlProcess:fly_state = 0\n");
            }
            else if (packet.data[2] == 2)
            {
                // 配置加速
                mine_fly_control_set_fly_state(2);
                printf("mine_remoterCtrlProcess:fly_state = 2\n");
            }
            else if (packet.data[2] == 3)
            {
                // 配置减速
                mine_fly_control_set_fly_state(3);
                printf("mine_remoterCtrlProcess:fly_state = 3\n");
            }
            else
            {
                printf("mine_remoterCtrlProcess:fly_state = %c\n", packet.data[2]);
            }
        }
    }

    // mine_printfPacket(pk);
    // if (pk->data[0] == REMOTER_CMD)
    // {
    //     switch (pk->data[1])
    //     {
    //         // case CMD_FLIGHT_LAND:
    //         //     if (getCommanderKeyFlight() != true)
    //         //     {
    //         //         setCommanderKeyFlight(true);
    //         //         setCommanderKeyland(false);
    //         //     }
    //         //     else
    //         //     {
    //         //         setCommanderKeyFlight(false);
    //         //         setCommanderKeyland(true);
    //         //     }
    //         //     break;

    //         // case CMD_EMER_STOP:
    //         //     setCommanderKeyFlight(false);
    //         //     setCommanderKeyland(false);
    //         //     break;

    //         // case CMD_FLIP:
    //         //     setFlipDir(pk->data[2]);
    //         //     break;

    //     case CMD_GET_MSG:
    //         mine_sendMsgACK();
    //         break;

    //         // case CMD_POWER_MODULE:
    //         //     expModulePower(pk->data[2]);
    //         //     break;

    //         // case CMD_LEDRING_EFFECT:
    //         //     //				expModulePower(true);
    //         //     setLedringEffect(pk->data[2]);
    //         //     break;

    //         // case CMD_POWER_VL53LXX:
    //         //     setVl53lxxState(pk->data[2]);
    //         //     break;
    //     }
    // }
    // else if (pk->data[0] == REMOTER_DATA)
    // {
    //     remoterData_t remoterData = *(remoterData_t *)(pk->data + 1);

    //     remoterCtrl.roll = remoterData.roll;
    //     remoterCtrl.pitch = remoterData.pitch;
    //     remoterCtrl.yaw = remoterData.yaw;
    //     remoterCtrl.thrust = remoterData.thrust * 655.35f;
    //     remoterCtrl.trimPitch = remoterData.trimPitch;
    //     remoterCtrl.trimRoll = remoterData.trimRoll;

    //     setCommanderCtrlMode(remoterData.ctrlMode);
    //     setCommanderFlightmode(remoterData.flightMode);
    //     flightCtrldataCache(ATK_REMOTER, remoterCtrl);
    // }
}
